#include "simstruc.h"
#include "tmwtypes.h"
#include <libserialport.h>
#include <pthread.h>
#include <stdio.h>
#include <unistd.h> 
#include "os/types.h"
#include "controller/motor.h"


FILE * fp_debug;

void msleep(u32 ms) {
    useconds_t us = ms * 1000;
    usleep(us);
}

void sim_sfunc_terminate(SimStruct *S) {
    ssPrintf("Stop\n");
    msleep(100);
    if (fp_debug) {
        fflush(fp_debug);
        fclose(fp_debug);
    }
}

void sim_sfunc_init(SimStruct *S) {
    if (fp_debug == NULL) {
        fp_debug = fopen("debug.txt", "w+");
    }
    motor_init(motor(0));
}